#include "../utility.h"
#include "mdio.h"

void TriMode_PhySetMdioDivisor(u8 Divisor) {
    gw_WriteReg(REG_MDIO_SETUP, (u32) Divisor | XAE_MDIO_MC_MDIOEN_MASK);
}


u16 TriMode_PhyRead(u32 PhyAddress, u32 RegisterNum) {
	u32 MdioCtrlReg = 0;
	u32 value = 0U;
	volatile int32_t TimeoutLoops;

	/*
	 * Wait till MDIO interface is ready to accept a new transaction.
	 */
	TimeoutLoops = Xil_poll_timeout(Xil_In32, REG_MDIO_CONTROL, value,(value&XAE_MDIO_MCR_READY_MASK)!=0,
			   XAE_RST_DEFAULT_TIMEOUT_VAL);
	if(-1 == TimeoutLoops) {
		Xil_AssertVoidAlways();
	}

	MdioCtrlReg =   ((PhyAddress << XAE_MDIO_MCR_PHYAD_SHIFT) &
			XAE_MDIO_MCR_PHYAD_MASK) |
			((RegisterNum << XAE_MDIO_MCR_REGAD_SHIFT)
			& XAE_MDIO_MCR_REGAD_MASK) |
			XAE_MDIO_MCR_INITIATE_MASK |
			XAE_MDIO_MCR_OP_READ_MASK;

	gw_WriteReg(REG_MDIO_CONTROL, MdioCtrlReg);


	/*
	 * Wait till MDIO transaction is completed.
	 */
	TimeoutLoops = Xil_poll_timeout(Xil_In32, REG_MDIO_CONTROL, value, (value&XAE_MDIO_MCR_READY_MASK)!=0,
		   XAE_RST_DEFAULT_TIMEOUT_VAL);
	if(-1 == TimeoutLoops) {
		Xil_AssertVoidAlways();
	}

	/* Read data */
	u16 PhyData = Xil_In16(REG_MDIO_READ_DATA);
	return PhyData;	
}

void TriMode_PhyWrite(u32 PhyAddress, u32 RegisterNum, u16 PhyData) {
	u32 MdioCtrlReg = 0;
	u32 value=0U;
	volatile int32_t TimeoutLoops;

	/*
	 * Wait till MDIO interface is ready to accept a new transaction.
	 */
	TimeoutLoops = Xil_poll_timeout(Xil_In32, REG_MDIO_CONTROL, value,(value&XAE_MDIO_MCR_READY_MASK)!=0,
			   XAE_RST_DEFAULT_TIMEOUT_VAL);
	if(-1 == TimeoutLoops) {
		Xil_AssertVoidAlways();
	}

	MdioCtrlReg =   ((PhyAddress << XAE_MDIO_MCR_PHYAD_SHIFT) &
			XAE_MDIO_MCR_PHYAD_MASK) |
			((RegisterNum << XAE_MDIO_MCR_REGAD_SHIFT) &
			XAE_MDIO_MCR_REGAD_MASK) |
			XAE_MDIO_MCR_INITIATE_MASK |
			XAE_MDIO_MCR_OP_WRITE_MASK;

	gw_WriteReg(REG_MDIO_WRITE_DATA, PhyData);
	gw_WriteReg(REG_MDIO_CONTROL, MdioCtrlReg);

	/*
	 * Wait till the MDIO interface is ready to accept a new transaction.
	 */
	TimeoutLoops = Xil_poll_timeout(Xil_In32, REG_MDIO_CONTROL, value,(value&XAE_MDIO_MCR_READY_MASK)!=0,
		   XAE_RST_DEFAULT_TIMEOUT_VAL);
	if(-1 == TimeoutLoops) {
		Xil_AssertVoidAlways();
	}

}

/* 
	mdio_read 0x1 0x1 -- external status 0x796d (link up , 自协商完成)
	mdio_read 0x2 0x1 -- internal status 0x1ec  ok
*/

#define PAGE_ADDR 0x16
#define CFG_REG_2 0x0011
#define CFG_REG_1 0x0010
#define CFG_REG_3 0x0007
#define M88E1512_MODE 0x0014
#define COPPER_CONTROL_REG 0x00

void config_SGMII () {

	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, PAGE_ADDR, 0x00FF);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_2, 0x214B);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_1, 0x2144);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_2, 0x0C28);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_1, 0x2146);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_2, 0xB233);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_1, 0x214D);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_2, 0xCC0C);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_1, 0x2159);
	/* Switch to PHY page 0xFB. */
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, PAGE_ADDR, 0x00FB);
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, CFG_REG_3, 0x000D);
	/* Switch to PHY page 0x12. */
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, PAGE_ADDR, 0x12);
	/* Change mode to SGMII-to-Copper */
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, M88E1512_MODE, 0x8001);
	/* Return the PHY to page 0. */
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, PAGE_ADDR, 0);

	/* PHY software reset */
	u16 value = TriMode_PhyRead(EXTERNAL_PHY_ADDR, COPPER_CONTROL_REG);
	value = value | 0x8000;
	TriMode_PhyWrite(EXTERNAL_PHY_ADDR, COPPER_CONTROL_REG, value);

	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x16, 0x12);
	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x14, 0x8001);
	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x16, 0x12);
	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x14, 0x1);
	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x14, 0x8201);
	// TriMode_PhyWrite(EXTERNAL_PHY_ADDR, 0x16, 0x0);

	/* check link up in external phy and internal phy */

	// u16 TriMode_PhyRead(EXTERNAL_PHY_ADDR, u32 RegisterNum);

}
